import ConvexGeometry from "./convex-geometry";
import CONSTANT from '../constant';

export default class CapsuleGeometry extends ConvexGeometry{
    constructor(radius,halfHeight) {
        super(CONSTANT.GEOMETRY_TYPE_CAPSULE);
        this._radius = radius;
        this._halfHeight = halfHeight;
        this._gjkMargin = this._radius;
        this._updateMass();
    }

    //...........................................public................................................

    getRadius() {
        return this._radius;
    }
    getHalfHeight() {
        return this._halfHeight;
    }

    //...........................................private................................................

    _updateMass() {
        let r=this._radius,hh=this._halfHeight;
        let r2 = r * r;
        let hh2 = hh * hh;
        let cylinderVolume = 6.28318530717958 * r2 * hh;
        let sphereVolume = 3.14159265358979 * r2 * r * 4 / 3;
        this._volume = cylinderVolume + sphereVolume;
        let invVolume = this._volume === 0 ? 0 : 1 / this._volume;
        let inertiaXZ = invVolume * (cylinderVolume * (r2 * 0.25 + hh2 / 3) + sphereVolume * (r2 * 0.4 + hh * r * 0.75 + hh2));
        let icf=this.inertiaCoeff;
        icf[0] = inertiaXZ;
        icf[1] = 0;
        icf[2] = 0;
        icf[3] = 0;
        icf[4] = invVolume * (cylinderVolume * r2 * 0.5 + sphereVolume * r2 * 0.4);
        icf[5] = 0;
        icf[6] = 0;
        icf[7] = 0;
        icf[8] = inertiaXZ;
    }
    _computeAabb(_aabb,_tf) {
        let tf=_tf.elements,aabb=_aabb.elements,r=this._radius,hh=this._halfHeight;
        let radVecX = r,radVecY = r,radVecZ = r;
        let axisX = tf[4],axisY = tf[7],axisZ = tf[10];
        if(axisX < 0) axisX = -axisX;
        if(axisY < 0) axisY = -axisY;
        if(axisZ < 0) axisZ = -axisZ;
        axisX = axisX * hh;axisY = axisY * hh;axisZ = axisZ * hh;
        radVecX += axisX;radVecY += axisY;radVecZ += axisZ;
        aabb[0] = tf[0] - radVecX;aabb[1] = tf[1] - radVecY;aabb[2] = tf[2] - radVecZ;
        aabb[3] = tf[0] + radVecX;aabb[4] = tf[1] + radVecY;aabb[5] = tf[2] + radVecZ;
    }
    computeLocalSupportingVertex(dir,out) {
        let oe=out.elements;
        if(dir.y > 0) {
            oe[1]=this._halfHeight;
            oe[0]=oe[2]=0;
        } else {
            oe[1]=-this._halfHeight;
            oe[0]=oe[2]=0;
        }
    }
    _rayCastLocal(beginX,beginY,beginZ,endX,endY,endZ,hit) {
        let halfH = this._halfHeight;
        let dx = endX - beginX;
        let dz = endZ - beginZ;
        let tminxz = 0;
        let tmaxxz;
        let a = dx * dx + dz * dz;
        let b = beginX * dx + beginZ * dz;
        let c = beginX * beginX + beginZ * beginZ - this._radius * this._radius;
        let D = b * b - a * c;
        if(D < 0) {
            return false;
        }
        if(a > 0) {
            let sqrtD = Math.sqrt(D);
            tminxz = (-b - sqrtD) / a;
            tmaxxz = (-b + sqrtD) / a;
            if(tminxz >= 1 || tmaxxz <= 0) {
                return false;
            }
        } else {
            if(c >= 0) {
                return false;
            }
            tminxz = 0;
        }
        let crossY = beginY + (endY - beginY) * tminxz;
        let min;
        if(crossY > -halfH && crossY < halfH) {
            if(tminxz > 0) {
                min = tminxz;
                let ne=hit.normal.elements;
                ne[0]=beginX + dx * min;ne[1]=0;ne[2]=beginZ + dz * min;
                let invLen = Math.sqrt(ne[0] * ne[0] + ne[1] * ne[1] + ne[2] * ne[2]);
                if(invLen > 0) {
                    invLen = 1 / invLen;
                }
                ne[0] *= invLen;ne[1] *= invLen;ne[2] *= invLen;
                let pe=hit.position.elements;
                pe[0]=beginX + min * dx;pe[1]=crossY;pe[2]=beginZ + min * dz;
                hit.fraction = min;
                return true;
            }
            return false;
        }
        let spherePosX = 0,spherePosY = crossY < 0 ? -halfH : halfH,spherePosZ = 0;
        let sphereToBeginX = beginX - spherePosX,sphereToBeginY = beginY - spherePosY,sphereToBeginZ = beginZ - spherePosZ;
        let dX = endX - beginX,dY = endY - beginY,dZ = endZ - beginZ;
        a = dX * dX + dY * dY + dZ * dZ;
        b = sphereToBeginX * dX + sphereToBeginY * dY + sphereToBeginZ * dZ;
        c = sphereToBeginX * sphereToBeginX + sphereToBeginY * sphereToBeginY + sphereToBeginZ * sphereToBeginZ - this._radius * this._radius;
        D = b * b - a * c;
        if(D < 0) {
            return false;
        }
        let t = (-b - Math.sqrt(D)) / a;
        if(t < 0 || t > 1) {
            return false;
        }
        let hitPosX = sphereToBeginX + dX * t,hitPosY = sphereToBeginY + dY * t,hitPosZ = sphereToBeginZ + dZ * t;
        let l = hitPosX * hitPosX + hitPosY * hitPosY + hitPosZ * hitPosZ;
        if(l > 0) {
            l = 1 / Math.sqrt(l);
        }
        let hitNormalX = hitPosX * l,hitNormalY = hitPosY * l,hitNormalZ = hitPosZ * l;
        hitPosX += spherePosX;hitPosY += spherePosY;hitPosZ += spherePosZ;
        let v = hit.position.elements;
        v[0] = hitPosX;v[1] = hitPosY;v[2] = hitPosZ;
        let v1 = hit.normal.elements;
        v1[0] = hitNormalX;v1[1] = hitNormalY;v1[2] = hitNormalZ;
        hit.fraction = t;
        return true;
    }
}